#!/usr/bin/env python3
from launch import LaunchDescription
from launch.actions import IncludeLaunchDescription, TimerAction
from launch.launch_description_sources import PythonLaunchDescriptionSource
from launch_ros.actions import Node
from launch.substitutions import Command, PathJoinSubstitution
from launch_ros.parameter_descriptions import ParameterValue
from launch_ros.substitutions import FindPackageShare
import yaml

def load_yaml(package_name, file_path):
    """加载YAML文件"""
    from ament_index_python.packages import get_package_share_directory
    import os
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)
    
    with open(absolute_file_path, 'r') as file:
        return yaml.safe_load(file)

def generate_launch_description():
    armbot_pkg = FindPackageShare('armbot')
    moveit_config_pkg = FindPackageShare('armbot_config')
    
    # URDF
    urdf_file = PathJoinSubstitution([armbot_pkg, 'urdf', 'armbot.urdf.xacro'])
    robot_description = ParameterValue(Command(['xacro ', urdf_file]), value_type=str)
    
    # SRDF
    srdf_file = PathJoinSubstitution([moveit_config_pkg, 'config', 'armbot.srdf'])
    robot_description_semantic = ParameterValue(Command(['cat ', srdf_file]), value_type=str)
    
    # 加载YAML配置
    kinematics_yaml = load_yaml('armbot_config', 'config/kinematics.yaml')
    ompl_yaml = load_yaml('armbot_config', 'config/ompl_planning.yaml')
    moveit_controllers_yaml = load_yaml('armbot_config', 'config/moveit_controllers.yaml')
    
    # MoveIt配置
    moveit_config = {
        'robot_description': robot_description,
        'robot_description_semantic': robot_description_semantic,
        'robot_description_kinematics': kinematics_yaml,
        'planning_pipelines': ['ompl'],
        'ompl': ompl_yaml,
        'use_sim_time': True,
    }
    
    # 合并控制器配置到moveit_config
    moveit_config.update(moveit_controllers_yaml)
    
    # 启动Gazebo
    gazebo_launch = IncludeLaunchDescription(
        PythonLaunchDescriptionSource([
            PathJoinSubstitution([armbot_pkg, 'launch', 'gazebo.launch.py'])
        ])
    )
    
    # MoveIt MoveGroup（延迟启动）
    move_group = Node(
        package='moveit_ros_move_group',
        executable='move_group',
        output='screen',
        parameters=[moveit_config]
    )
    
    delayed_move_group = TimerAction(
        period=5.0,
        actions=[move_group]
    )
    
    # RViz
    rviz_config = PathJoinSubstitution([moveit_config_pkg, 'config', 'moveit.rviz'])
    rviz = Node(
        package='rviz2',
        executable='rviz2',
        arguments=['-d', rviz_config],
        parameters=[moveit_config]
    )
    
    return LaunchDescription([
        gazebo_launch,
        delayed_move_group,
        rviz
    ])
